#include "WorldGeometry.h"

WorldGeometry::WorldGeometry() : mPolygonCount(0) {
}


void WorldGeometry::compile( const Polygon2* polys, size_t ct ) {
	mVertices.clear();
	mEdges.clear();
	for ( size_t idx = 0; idx < ct; ++idx )
		addPolygon( polys[idx] );
}

int WorldGeometry::addPolygon( const Polygon2& poly ) {
	const std::vector<Contour2>& contours = poly.contours();
	int ctId = 0;

	int id = mPolygonCount;
	mPolygonCount++;
	FOR ( const Contour2& contour, contours ) {
		const std::vector<Point2>& vertices = contour.vertices();
		const std::vector<Vector2>& normals = contour.edgeNormals();

		size_t idxBase = mVertices.size();
		mVertices.insert( mVertices.end(), vertices.begin(), vertices.end() );
		for ( size_t idxA = vertices.size()-1, idxB = 0; idxB < vertices.size(); idxA = idxB, ++idxB ) {
			IndexedEdge temp = { idxBase + idxA, idxBase + idxB, normals[idxA] };
			mEdges.push_back( temp );
			EdgeInfo info = { id, ctId, idxB };
			mEdgeInfo.push_back( info );
		}

		ctId ++;
	}

	return id;
}

bool WorldGeometry::trace( const Point2& pt, const Vector2& dir, IntersectionResult& result ) const {
	static const double epsilon = 1.0e-10;
	double bestDistance = std::numeric_limits<double>::max();
	const IndexedEdge* bestEdge = 0;
	int bestEdgeIdx = -1;
	Point2 bestPoint = pt;

	Vector2 vP = dir.lhNormal();
	int idxEdge = 0;
	FOR ( const IndexedEdge& edge, mEdges ) {
		const Point2& ptA = mVertices[edge.idxA];
		const Point2& ptB = mVertices[edge.idxB];
		Vector2 u = ptB - ptA;
		Vector2 w = ptA - pt;

		double numer = -vP.dot(w);
		double denom = vP.dot(u);
		if ( std::abs( denom ) < epsilon ) // Parallel
			continue;

		double s = numer / denom;
		if ( s < 0. || s > 1. ) // Outside of line segment
			continue;

		Point2 intersection = ptA + u*s;
		Vector2 delta = intersection - pt;
		double d = delta.sqLength();
		if ( d > epsilon && d < bestDistance && dir.dot(delta) >= 0 ) {
			bestDistance = d;
			bestEdgeIdx = idxEdge;
			bestEdge = &edge;
			bestPoint = intersection;
		}
		idxEdge ++;
	}

	// No intersection found
	if ( !bestEdge )
		return false;

	result.intersection = bestPoint;
	result.sqDistance = bestDistance;
	result.normal = bestEdge->normal;
	result.extra = &mEdgeInfo[bestEdgeIdx];
	return true;
}
